#pragma once
#include "vector3.h"

namespace krryn{
namespace math{
	template<typename T>
	class quaternion{
	public:
		T m_X, m_Y, m_Z, m_W;

		quaternion()
			: m_X(0), m_Y(0), m_Z(0), m_W(0) {}

		quaternion(T a_X, T a_Y, T a_Z, T a_W)
			: m_X(a_X), m_Y(a_Y), m_Z(a_Z), m_W(a_W) {}

		static quaternion implicit_w(T a_X, T a_Y, T a_Z){
			T l_Temp = 1 - (a_X * a_X) - (a_Y * a_Y) - (a_Z * a_Z);

			if(l_Temp < 0) l_Temp = 0;
			else l_Temp = -math::sqrt(l_Temp);

			return quaternion(a_X, a_Y, a_Z, l_Temp);
		}
		
		quaternion normalize() const{
			T l_Length = math::sqrt((m_X * m_X) + (m_Y * m_Y) + (m_Z * m_Z) + (m_W * m_W));

			if(l_Length > 0){
				l_Length = 1 / l_Length;
				return quaternion(
					m_X * l_Length, 
					m_Y * l_Length, 
					m_Z * l_Length, 
					m_W * l_Length
				);
			}else{
				return quaternion::zero();
			}
		}

		static quaternion zero(){
			return quaternion(0, 0, 0, 0);
		}

		quaternion operator-() const{
			return quaternion(
				-m_X,
				-m_Y,
				-m_Z,
				 m_W // generally not inverted
			);
		}

		quaternion& operator=(const quaternion &a_Other){
			m_X = a_Other.m_X;
			m_Y = a_Other.m_Y;
			m_Z = a_Other.m_Z;
			m_W = a_Other.m_W;

			return *this;
		}

		quaternion& operator*=(const quaternion &a_Other){
			*this = *this * a_Other;
			return *this;
		}


		quaternion operator*(const quaternion &a_Other) const{
			return quaternion(
				(m_X * a_Other.m_W) + (m_W * a_Other.m_X) + (m_Y * a_Other.m_Z) - (m_Z * a_Other.m_Y),
				(m_Y * a_Other.m_W) + (m_W * a_Other.m_Y) + (m_Z * a_Other.m_X) - (m_X * a_Other.m_Z),
				(m_Z * a_Other.m_W) + (m_W * a_Other.m_Z) + (m_X * a_Other.m_Y) - (m_Y * a_Other.m_X),
				(m_W * a_Other.m_W) - (m_X * a_Other.m_X) - (m_Y * a_Other.m_Y) - (m_Z * a_Other.m_Z)
			);
		}

		template<typename _Tag>
		quaternion operator*(const vector3<T, _Tag> &a_Vector) const{
			return quaternion(
				  (m_W * a_Vector.m_X) + (m_Y * a_Vector.m_Z) - (m_Z * a_Vector.m_Y),
				  (m_W * a_Vector.m_Y) + (m_Z * a_Vector.m_X) - (m_X * a_Vector.m_Z),
				  (m_W * a_Vector.m_Z) + (m_X * a_Vector.m_Y) - (m_Y * a_Vector.m_X),
				- (m_X * a_Vector.m_X) - (m_Y * a_Vector.m_Y) - (m_Z * a_Vector.m_Z)
			);
		}

		template<typename _Tag>
		vector3<T, _Tag> rotate(const vector3<T, _Tag> &a_Vector) const{
			quaternion l_Inverse = (-*this).normalize();
			quaternion l_Temp    = *this * a_Vector;
			quaternion l_Result  = l_Temp * l_Inverse;

			return vector3<T, _Tag>(
				l_Result.m_X,
				l_Result.m_Y,
				l_Result.m_Z
			);
		}

		float dot(const quaternion &a_Q2) const{
			return m_X * a_Q2.m_X + m_Y * a_Q2.m_Y + m_Z * a_Q2.m_Z + m_W * a_Q2.m_W;
		}

		quaternion  lerp(const quaternion &a_Q2, float t) const{
			return quaternion(
				(m_X * (1 - t)) + (a_Q2.m_X * t),
				(m_Y * (1 - t)) + (a_Q2.m_Y * t),
				(m_Z * (1 - t)) + (a_Q2.m_Z * t),
				(m_W * (1 - t)) + (a_Q2.m_W * t));
		};

		//matrix as_matrix() const{
		//	// TODO: This is untested
		//	matrix m;

		//	m.cell[0] = 1 - (2 * y * y) - (2 * z * z);
		//	m.cell[1] = (2 * x * y) + (2 * w * z);
		//	m.cell[2] = (2 * x * z) - (2 * w * y);

		//	m.cell[4] = (2 * x * y) - (2 * w * z);
		//	m.cell[5] = 1 - (2 * x * x) - (2 * z * z);
		//	m.cell[6] = (2 * y * z) + (2 * w * x);

		//	m.cell[8] = (2 * x * z) + (2 * w * y);
		//	m.cell[9] = (2 * y * x) - (2 * w * x);
		//	m.cell[10]= 1 - (2 * x * x) - (2 * y * y);

		//	m.cell[3] 
		//		= m.cell[7]  = m.cell[11] = m.cell[12]
		//		= m.cell[13] = m.cell[14] = m.cell[15] = 0;

		//	return m;
		//}

		quaternion nlerp(const quaternion &a_Q2, float t) const{
			return lerp(a_Q2, t).normalize();
		};

		quaternion slerp(const quaternion &a_Q2, float t) const{
			/* Check for out-of range parameter and return edge points if so */
			if(t <= 0.0){
				return *this;
			}

			if(t >= 1.0){
				return a_Q2;
			}

			/* Compute "cosine of angle between quaternions" using dot product */
			float cosOmega = dot(a_Q2);

			/* If negative dot, use -q1.  Two quaternions q and -q
			represent the same rotation, but may produce
			different slerp.  We chose q or -q to rotate using
			the acute angle. */
			quaternion temp = a_Q2;

			if(cosOmega < 0.0f){
				temp     = -temp;
				temp.m_W = -temp.m_W;
				cosOmega = -cosOmega;
			}

			/* We should have two unit quaternions, so dot should be <= 1.0 */
			assert (cosOmega < 1.1f);

			/* Compute interpolation fraction, checking for quaternions
			almost exactly the same */
			float k0, k1;

			if(cosOmega > 0.9999f){
				/* Very close - just use linear interpolation,
				which will protect againt a divide by zero */

				k0 = 1.0f - t;
				k1 = t;
			}else{
				/* Compute the sin of the angle using the
				trig identity sin^2(omega) + cos^2(omega) = 1 */
				float sinOmega = math::sqrt(1.0f - (cosOmega * cosOmega));

				/* Compute the angle from its sin and cosine */
				float omega = math::atan2(sinOmega, cosOmega);

				/* Compute inverse of denominator, so we only have
				to divide once */
				float oneOverSinOmega = 1.0f / sinOmega;

				/* Compute interpolation parameters */
				k0 = math::sin((1.0f - t) * omega) * oneOverSinOmega;
				k1 = math::sin(t * omega) * oneOverSinOmega;
			}

			/* Interpolate and return new quaternion */
			return quaternion(
				(k0 * m_X) + (k1 * temp.m_X),
				(k0 * m_Y) + (k1 * temp.m_Z),
				(k0 * m_Z) + (k1 * temp.m_Y),
				(k0 * m_W) + (k1 * temp.m_W));
		}
	};

	typedef quaternion<float> quaternionf;
}
}